/**=============================================================================
@file
   qhdsp_hvx_float_fft_real.c

@brief
   HVX implementation of complex FFT in C [float].

Copyright (c) 2020 Qualcomm Technologies Incorporated.
All Rights Reserved. Qualcomm Proprietary and Confidential.
=============================================================================**/

#if __HVX_ARCH__ >= 68

#include "qhdsp_hvx.h"
#include "qhdsp_hvx_fft_internal.h"
#include "qhdsp_hvx_float_fft_internal.h"
#include "hvx_fft_common.h"

#ifndef VLEN
#define VLEN 128
#endif


#define   creal_f(x)      (qhcomplex_creal_f( (x) ) )
#define   cimag_f(x)      (qhcomplex_cimag_f( (x) ) )

/**
 * @brief           [HVX] Real 1D 2^N single precision float point FFT
 * @param[in]       input - input samples in time domain
 * @param[in]       N - number of points on which FFT is performed
 * @param[in]       w1 - twiddle factors - for N/2-point complex FFT
 * @param[in]       w2 - twiddle factors - for last stage
 * @param[out]      output - FFT output buffer
 * @note
 *                  - Assumptions:
 *                                  1. input, w1, w2, output - buffer aligned by VLENbytes
 *                                  2. w1, w2 - generated with qhdsp_hvx_fft_gen_twiddles_real_vcf() function
 *                                  3. N is power of 2 and N>=64
 */
int32_t qhdsp_hvx_r1dfft_af(const float* input, uint32_t N, const float complex *Wt1, const float complex *Wt2, float complex *output)
{
    float complex X, Y;
    float complex *Z = output;

    // Construct complex array with even input as real and odd input as imaginary parts
    // Then do N/2-point complex FFT
    qhdsp_hvx_c1dfft_af((float complex *)input, N / 2, Wt1, Z);

    /*************************************/
    /*  Calculate last stage butterflies */
    /**************************************/
    // calculate FFT at k=0, k=N/2

    float complex output_0_backup;
    float complex output_N_4_backup;

    output_0_backup = output[0];
    output_N_4_backup = output[N/4];

    HVX_VP V1__0, V3__2, V5__4, V7__6, V9__8, V11__10, V13__12, V15__14;
    HVX_Vector previous_output_0 = Q6_V_vzero();

    V4 = Q6_Vb_vsplat_R(0x78);                      // VDELTA control for reversing 8-byte words in a vector
    V12 = Q6_V_vsplat_R(0x80000000);                // mask for negation - done with XOR operation

    V14 = Q6_V_vsplat_R(0x3f000000);                // 0.5 in IEEE float
    V13 = Q6_V_vzero();                             // 0.0 * i
    V15_14 = Q6_W_vshuff_VVR(V13, V14, -4);         // shuffle real & imag parts

    HVX_Vector *out_out_0  = (HVX_Vector *)(&output[0]);
    HVX_Vector *out_in_N_2 = (HVX_Vector *)(&output[N / 2]);
    out_in_N_2--;       // move back pointer for first read

    HVX_Vector *twiddles   = (HVX_Vector *)Wt2;

    for (uint32_t i = 0; i < N / 4; i+=32)
    {
        V0 = Q6_V_vdelta_VV(*out_in_N_2--, V4);
        V1 = Q6_V_vdelta_VV(*out_in_N_2++, V4);
        V3_2 = Q6_W_vdeal_VVR(V1, V0, -4);          // split to real & imag parts (output[N/2 - i])
        V3 = Q6_V_vxor_VV(V3, V12);                 // negate imag part - conjugate
        V3_2 = Q6_W_vshuff_VVR(V3, V2, -4);         // shuffle back real & imag parts

        V0 = *out_out_0++;                          // output[i]; i:  0-15
        V1 = *out_out_0++;                          // output[i]; i: 16-31
        V5 = *out_out_0--;                          // output[i]; i: 32-47

        V0 = Q6_V_valign_VVR(V1, V0, 8);            // output[i]; i:  1-16
        V1 = Q6_V_valign_VVR(V5, V1, 8);            // output[i]; i: 17-32

        V6 = Q6_Vqf32_vadd_VsfVsf(V0, V2);          // X.real = Z[i] + conj_f(Z[N / 2 - i]);
        V7 = Q6_Vqf32_vadd_VsfVsf(V1, V3);          // X.imag = Z[i] + conj_f(Z[N / 2 - i]);
        V6 = Q6_Vsf_equals_Vqf32(V6);
        V7 = Q6_Vsf_equals_Vqf32(V7);

        V8 = Q6_Vqf32_vsub_VsfVsf(V0, V2);          // Y.real = Z[i] - conj_f(Z[N / 2 - i]);
        V9 = Q6_Vqf32_vsub_VsfVsf(V1, V3);          // Y.imag = Z[i] - conj_f(Z[N / 2 - i]);
        V8 = Q6_Vsf_equals_Vqf32(V8);
        V9 = Q6_Vsf_equals_Vqf32(V9);

        qf32_V_CPLX_MULT_sf_sf(V7_6, V15_14, &V7_6);// X = 0.5 * X;

        V10 = *twiddles++;
        V11 = *twiddles++;

        qf32_V_CPLX_MULT_sf_sf(V9_8, V11_10, &V9_8);// Y = Wt2[i - 1] * Y;

        out_out_0--;

        V0 = Q6_Vqf32_vadd_Vqf32Vqf32(V6, V8);      // X + Y; - 1st 16 output values
        V1 = Q6_Vqf32_vadd_Vqf32Vqf32(V7, V9);      // X + Y; - 2nd 16 output values
        V2 = Q6_V_valign_VVR(V0, previous_output_0, 120);
        V3 = Q6_V_valign_VVR(V1, V0, 120);
        previous_output_0 = V1;
        *out_out_0++ = Q6_Vsf_equals_Vqf32(V2);     // convert to IEEE float; output[i] = X + Y;
        *out_out_0++ = Q6_Vsf_equals_Vqf32(V3);     // convert to IEEE float; output[i] = X + Y;

        V10 = Q6_Vqf32_vsub_Vqf32Vqf32(V6, V8);     // X - Y; - 1st 16 output values
        V11 = Q6_Vqf32_vsub_Vqf32Vqf32(V7, V9);     // X - Y; - 2nd 16 output values
        V11_10 = Q6_W_vdeal_VVR(V11, V10, -4);      // split to real & imag parts

        V11 = Q6_Vsf_equals_Vqf32(V11);         // HACK/WORKAROUND! -   for some reason, setting bit 31. in qf32 doesn't work properly
                                                //                      thus, converting to IEEE float
        V11 = Q6_V_vxor_VV(V11, V12);               // negate imag part - conjugate: conj_f(X - Y);
        V11 = Q6_Vqf32_vadd_VsfVsf(V11, V13);   // HACK/WORKAROUND! -   back to qf32

        V1_0 = Q6_W_vshuff_VVR(V11, V10, -4);       // shuffle back real & imag parts

        V0 = Q6_V_vdelta_VV(V0, V4);
        V1 = Q6_V_vdelta_VV(V1, V4);

        *out_in_N_2-- = Q6_Vsf_equals_Vqf32(V0);    // convert to IEEE float; output[N / 2 - i] = conj_f(X - Y);
        *out_in_N_2-- = Q6_Vsf_equals_Vqf32(V1);    // convert to IEEE float; output[N / 2 - i] = conj_f(X - Y);
    }

    X = creal_f(output_0_backup) + 1i * 0;
    Y = cimag_f(output_0_backup) + 1i * 0;
    output[0] = X + Y;

    /*
        output[N / 4] calculation:

        X = Z[N / 4] + conj_f(Z[N / 2 - N / 4]) = Z[N / 4] + conj_f(Z[N / 4]) = 2 * Z[N / 4].re + 1i * 0;
        Y = Z[N / 4] - conj_f(Z[N / 2 - N / 4]) = Z[N / 4] - conj_f(Z[N / 4]) = 0.0 + 1i * 2.0 * Z[N / 4].im;
        X = 0.5 * X = Z[N / 4].re + 1i * 0;
        Y = Wt2[i - 1] * Y;
        output[i] = X + Y;
    */

    X = creal_f(output_N_4_backup) + 1i * 0;
    Y = 0.0 + 2.0 * 1i * cimag_f(output_N_4_backup);
    Y = Y * Wt2[N/4-1];
    output[N / 4] = X + Y;

    HVX_VectorPair *out_out_N_2 = (HVX_VectorPair *)(&output[N / 2]);
    HVX_Vector *out_in_N   = (HVX_Vector *)(&output[N/2]);

    previous_output_0 = *out_in_N--;                // output[i]; i: N/2 : N/2+15

    for (uint32_t i = N / 2; i < N; i+=32)
    {
        V0 = *out_in_N--;                           // output[i]; i: N/2-16 : N/2-1
        V1 = *out_in_N--;                           // output[i]; i: N/2-32 : N/2-17

        V2 = Q6_V_valign_VVR(previous_output_0, V0, 8); // output[i]; i: N/2-15 : N/2
        V3 = Q6_V_valign_VVR(V0, V1, 8);            // output[i]; i: N/2-31 : N/2-16
        V2 = Q6_V_vdelta_VV(V2, V4);                // output[i]; i: N/2 : N/2-15
        V3 = Q6_V_vdelta_VV(V3, V4);                // output[i]; i: N/2-16 : N/2-31
        previous_output_0 = V1;

        V3_2 = Q6_W_vdeal_VVR(V3, V2, -4);          // split to real & imag parts (output[N/2 - i])
        V3 = Q6_V_vxor_VV(V3, V12);                 // negate imag part - conjugate
        V1_0 = Q6_W_vshuff_VVR(V3, V2, -4);         // shuffle back real & imag parts

        *out_out_N_2++ = V1_0;
    }

    X = creal_f(output_0_backup) + 1i * 0;
    Y = cimag_f(output_0_backup) + 1i * 0;
    output[N / 2] = X - Y;

    return 0;
}

/**
 * @brief           [HVX] Real 1D 2^N single precision float point IFFT
 * @param[in]       input - input samples in frequency domain
 * @param[in]       n - number of samples on which IFFT is performed
 * @param[in]       Wt1 - twiddle factors - for N/2-point complex IFFT
 * @param[in]       Wt2 - twiddle factors - for last stage
 * @param[out]      output - IFFT output buffer
 * @note
 *                  - Assumptions:
 *                                  1. input, w1, w2, output - buffer aligned by VLENbytes
 *                                  2. w1, w2 - generated with qhdsp_hvx_fft_gen_twiddles_real_vcf() function
 *                                  3. N is power of 2 and N>=64
 */
int32_t qhdsp_hvx_r1difft_af(const float complex *input, uint32_t N, const float complex *Wt1, const float complex *Wt2, float *output)
{
    float complex *c1difft_sf_input_buff;

    const int32_t byte_alignment = VLEN * sizeof(uint8_t);
    if(N/2 * sizeof(float complex) + (byte_alignment-1) > 2048)
    {
        c1difft_sf_input_buff = (float complex *)memalign(byte_alignment, N/2*sizeof(float complex));
    }
    else
    {
        c1difft_sf_input_buff = (float complex *)__builtin_alloca(N/2*sizeof(int64_t) + (byte_alignment-1));
        int32_t offset = byte_alignment - 1;
        c1difft_sf_input_buff = (float complex *)(((int32_t)c1difft_sf_input_buff + offset) & ~(byte_alignment-1));
    }

    if(c1difft_sf_input_buff==NULL)
    {
        //printf("Unable to allocate temp buff, exiting...");
        return -1;
    }

    HVX_VP V1__0, V3__2, V5__4, V7__6, V9__8, V11__10, V13__12, V15__14;

    HVX_Vector *in_0      = (HVX_Vector *)(&input[0]);
    HVX_Vector *in_N_2    = (HVX_Vector *)(&input[N / 2]);
    HVX_VectorPair *temp_buff_ptr;
    HVX_Vector *twiddles  = (HVX_Vector *)Wt2;

    V12 = Q6_V_vsplat_R(0x80000000);                // mask for negation - done with XOR operation
    V0 = Q6_V_vzero();                              // 0.0
    V1 = Q6_V_vsplat_R(0xbf000000);                 // -0.5 in IEEE float
    V3_2 = Q6_W_vshuff_VVR(V1, V0, -4);             // shuffle real & imag parts together: 0.0 - 0.5 * i
    HVX_Vector previous_twiddles = V2;

    V1 = Q6_V_vsplat_R(0x3f000000);                 // 0.5 in IEEE float
    V15_14 = Q6_W_vshuff_VVR(V0, V1, -4);           // shuffle real & imag parts together: 0.5 + 0.0 * i

    temp_buff_ptr = (HVX_VectorPair *)c1difft_sf_input_buff;

    HVX_Vector previous_in_N_2 = *in_N_2--;         // input[N/2]    -> input[N/2+15];

    V10 = Q6_Vb_vsplat_R(0x78);                     // VDELTA control for reversing 8-byte words in a vector

    for (uint32_t i = 0; i < N / 2; i+=32)
    {
        V0 = *in_0++;                               // input[i]; i:  0-15
        V1 = *in_0++;                               // input[i]; i:  16-31

        V2 = *in_N_2--;                             // input[N/2-16] -> input[N/2-1]
        V3 = *in_N_2--;                             // input[N/2-32] -> input[N/2-17]

        V4 = Q6_V_valign_VVR(previous_in_N_2, V2, 8); // input[N/2-15] -> input[N/2-0]
        V5 = Q6_V_valign_VVR(V2, V3, 8);              // input[N/2-31] -> input[N/2-16]
        previous_in_N_2 = V3;

        V2 = Q6_V_vdelta_VV(V4, V10);               // input[N/2-0]  -> input[N/2-15]
        V3 = Q6_V_vdelta_VV(V5, V10);               // input[N/2-16] -> input[N/2-31]

        V3_2 = Q6_W_vdeal_VVR(V3, V2, -4);          // V2 - real part, V3 - imag part
        V3 = Q6_V_vxor_VV(V3, V12);                 // negate imag part - conjugate
        V3_2 = Q6_W_vshuff_VVR(V3, V2, -4);         // shuffle back real & imag parts together
                                                    // V3_2 = conjugate(input[N/2-i]); i:  0-31

        V4 = Q6_Vqf32_vadd_VsfVsf(V0, V2);          // X = input[i] + conj_f(input[N/2-i]); - 1st 16 output values
        V5 = Q6_Vqf32_vadd_VsfVsf(V1, V3);          // X = input[i] + conj_f(input[N/2-i]); - 2nd 16 output values
        V4 = Q6_Vsf_equals_Vqf32(V4);
        V5 = Q6_Vsf_equals_Vqf32(V5);

        V6 = Q6_Vqf32_vsub_VsfVsf(V0, V2);          // Y = input[i] - conj_f(input[N/2-i]); - 1st 16 output values
        V7 = Q6_Vqf32_vsub_VsfVsf(V1, V3);          // Y = input[i] - conj_f(input[N/2-i]); - 2nd 16 output values
        V6 = Q6_Vsf_equals_Vqf32(V6);
        V7 = Q6_Vsf_equals_Vqf32(V7);

        qf32_V_CPLX_MULT_sf_sf(V5_4, V15_14, &V5_4);// X = 0.5 * X;

        V0 = *twiddles++;                           // w2[i];   i:  0-15
        V1 = *twiddles++;                           // w2[i];   i:  16-31

        V8 = Q6_V_valign_VVR(V0, previous_twiddles, 120);  // w2[i];   i:  1-31 & previous_twiddles (1 value)
        V9 = Q6_V_valign_VVR(V1, V0, 120);

        previous_twiddles = V1;

        qf32_V_CPLX_MULT_conj_sf_sf(V7_6, V9_8, &V7_6);  // Y = Y * conj_f(Wt2[i-1]);

        V0 = Q6_Vqf32_vadd_Vqf32Vqf32(V4, V6);      // X + Y;
        V1 = Q6_Vqf32_vadd_Vqf32Vqf32(V5, V7);      // X + Y;
        V0 = Q6_Vsf_equals_Vqf32(V0);
        V1 = Q6_Vsf_equals_Vqf32(V1);

        *temp_buff_ptr++ = V1_0;
    }

    qhdsp_hvx_c1difft_af(c1difft_sf_input_buff, N/2, Wt1, (float complex *)output);

    if(N/2 * sizeof(int64_t) + (byte_alignment-1) > 2048)
    {
        free(c1difft_sf_input_buff);
    }

    return 0;
}

/**
 * @brief           [HVX] Real 1D 2^N single precision float point IFFT - scratch
 *                  Temporary buffer provided from outside - improves performance
 *                  when called multiple times (such as in 2D).
 * @param[in]       input - input samples in frequency domain
 * @param[in]       n - number of samples on which IFFT is performed
 * @param[in]       Wt1 - twiddle factors - for N/2-point complex IFFT
 * @param[in]       Wt2 - twiddle factors - for last stage
 * @param[out]      output - IFFT output buffer
 * @param[in]       scratch_buff - temporary buffer (size=N/2*sizeof(float complex), alignment=VLENB)
 * @note
 *                  - Assumptions:
 *                                  1. input, w1, w2, output - buffer aligned by VLENbytes
 *                                  2. w1, w2 - generated with qhdsp_hvx_fft_gen_twiddles_real_vcf() function
 *                                  3. N is power of 2 and N>=64
 */
int32_t qhdsp_hvx_r1difft_af_scratch(const float complex *input, uint32_t N, const float complex *Wt1, const float complex *Wt2, float *output, float complex *scratch_buff)
{
    HVX_VP V1__0, V3__2, V5__4, V7__6, V9__8, V11__10, V13__12, V15__14;

    HVX_Vector *in_0      = (HVX_Vector *)(&input[0]);
    HVX_Vector *in_N_2    = (HVX_Vector *)(&input[N / 2]);
    HVX_VectorPair *temp_buff_ptr;
    HVX_Vector *twiddles  = (HVX_Vector *)Wt2;

    V12 = Q6_V_vsplat_R(0x80000000);                // mask for negation - done with XOR operation
    V0 = Q6_V_vzero();                              // 0.0
    V1 = Q6_V_vsplat_R(0xbf000000);                 // -0.5 in IEEE float
    V3_2 = Q6_W_vshuff_VVR(V1, V0, -4);             // shuffle real & imag parts together: 0.0 - 0.5 * i
    HVX_Vector previous_twiddles = V2;

    V1 = Q6_V_vsplat_R(0x3f000000);                 // 0.5 in IEEE float
    V15_14 = Q6_W_vshuff_VVR(V0, V1, -4);           // shuffle real & imag parts together: 0.5 + 0.0 * i

    temp_buff_ptr = (HVX_VectorPair *)scratch_buff;

    HVX_Vector previous_in_N_2 = *in_N_2--;         // input[N/2]    -> input[N/2+15];

    V10 = Q6_Vb_vsplat_R(0x78);                     // VDELTA control for reversing 8-byte words in a vector

    for (uint32_t i = 0; i < N / 2; i+=32)
    {
        V0 = *in_0++;                               // input[i]; i:  0-15
        V1 = *in_0++;                               // input[i]; i:  16-31

        V2 = *in_N_2--;                             // input[N/2-16] -> input[N/2-1]
        V3 = *in_N_2--;                             // input[N/2-32] -> input[N/2-17]

        V4 = Q6_V_valign_VVR(previous_in_N_2, V2, 8); // input[N/2-15] -> input[N/2-0]
        V5 = Q6_V_valign_VVR(V2, V3, 8);              // input[N/2-31] -> input[N/2-16]
        previous_in_N_2 = V3;

        V2 = Q6_V_vdelta_VV(V4, V10);               // input[N/2-0]  -> input[N/2-15]
        V3 = Q6_V_vdelta_VV(V5, V10);               // input[N/2-16] -> input[N/2-31]

        V3_2 = Q6_W_vdeal_VVR(V3, V2, -4);          // V2 - real part, V3 - imag part
        V3 = Q6_V_vxor_VV(V3, V12);                 // negate imag part - conjugate
        V3_2 = Q6_W_vshuff_VVR(V3, V2, -4);         // shuffle back real & imag parts together
                                                    // V3_2 = conjugate(input[N/2-i]); i:  0-31

        V4 = Q6_Vqf32_vadd_VsfVsf(V0, V2);          // X = input[i] + conj_f(input[N/2-i]); - 1st 16 output values
        V5 = Q6_Vqf32_vadd_VsfVsf(V1, V3);          // X = input[i] + conj_f(input[N/2-i]); - 2nd 16 output values
        V4 = Q6_Vsf_equals_Vqf32(V4);
        V5 = Q6_Vsf_equals_Vqf32(V5);

        V6 = Q6_Vqf32_vsub_VsfVsf(V0, V2);          // Y = input[i] - conj_f(input[N/2-i]); - 1st 16 output values
        V7 = Q6_Vqf32_vsub_VsfVsf(V1, V3);          // Y = input[i] - conj_f(input[N/2-i]); - 2nd 16 output values
        V6 = Q6_Vsf_equals_Vqf32(V6);
        V7 = Q6_Vsf_equals_Vqf32(V7);

        qf32_V_CPLX_MULT_sf_sf(V5_4, V15_14, &V5_4);// X = 0.5 * X;

        V0 = *twiddles++;                           // w2[i];   i:  0-15
        V1 = *twiddles++;                           // w2[i];   i:  16-31

        V8 = Q6_V_valign_VVR(V0, previous_twiddles, 120);  // w2[i];   i:  1-31 & previous_twiddles (1 value)
        V9 = Q6_V_valign_VVR(V1, V0, 120);

        previous_twiddles = V1;

        qf32_V_CPLX_MULT_conj_sf_sf(V7_6, V9_8, &V7_6);  // Y = Y * conj_f(Wt2[i-1]);

        V0 = Q6_Vqf32_vadd_Vqf32Vqf32(V4, V6);      // X + Y;
        V1 = Q6_Vqf32_vadd_Vqf32Vqf32(V5, V7);      // X + Y;
        V0 = Q6_Vsf_equals_Vqf32(V0);
        V1 = Q6_Vsf_equals_Vqf32(V1);

        *temp_buff_ptr++ = V1_0;
    }

    qhdsp_hvx_c1difft_af(scratch_buff, N/2, Wt1, (float complex *)output);

    return 0;
}

/**
 * @brief           [HVX] Real 2D (NxN) 2^N single precision float point FFT
 * @param[in]       input - input samples
 * @param[in]       N - number of points (of both dimensions) on which FFT is performed
 * @param[in]       w1 - twiddle factors - for N/2-point complex FFT
 * @param[in]       w2 - twiddle factors - for last stage
 * @param[out]      output - FFT output buffer
 * @note
 *                  - Assumptions:
 *                                  1. input, w1, w2, output - buffer aligned by VLENbytes
 *                                  2. w1, w2 - generated with qhdsp_hvx_2D_fft_gen_twiddles_real_vcf() function
 *                                  3. N is power of 2 and N>=64
 *                                  4. 2D FFT assumes width=height=stride -> N
 */
int32_t qhdsp_hvx_r2dfft_af(const float *input, uint32_t N, const float complex *w1, const float complex *w2, float complex *output)
{
    uint32_t LOG2N = ct0(N);

    if(N < 64 || N > 4096 || 1 << LOG2N != N)      // check size limits and if N is power of 2
    {
        return -1;
    }

    // temporary buffer for intermediate data storage
    float complex *temp_buff = (float complex *)memalign(VLEN, N*N*sizeof(float complex));
    float *input_ptr = (float *)input;

    if(temp_buff == NULL)
    {
        //printf("Unable to allocate temp buff... exiting.\n");
        return -1;
    }

    float complex *temp_buff_ptr = temp_buff;

    //                                stride        width        height
    uint32_t L2FETCH_REGISTER_ROWS = (VLEN << 16) | (VLEN << 8) | (N * 1/16);

    // first do all rows
    // place results in bit-reverse order
    for(uint32_t i=0; i<N; i++)
    {
        if(N-i > 1)
        {
            L2FETCH(input_ptr+N, L2FETCH_REGISTER_ROWS);
        }

        qhdsp_hvx_r1dfft_af(input_ptr, N, w1, w2, &temp_buff[bitrev(i, LOG2N)*N]);
        input_ptr += N;
    }

    // ...then all columns
    for(uint32_t i=0; i<N; i+=32)
    {
        qhdsp_hvx_c1dfft_column_af(temp_buff_ptr, N, w1, output);

        temp_buff_ptr += 32;
        output += 32;
    }

    free(temp_buff);

    return 0;
}

/**
 * @brief           [HVX] Real 2D (NxN) 2^N single precision float point IFFT
 * @param[in]       input - input samples
 * @param[in]       N - number of points (of both dimensions) on which IFFT is performed
 * @param[in]       w1 - twiddle factors - for N/2-point complex IFFT
 * @param[in]       w2 - twiddle factors - for last stage
 * @param[out]      output - IFFT output buffer
 * @note
 *                  - Assumptions:
 *                                  1. input, w1, w2, output - buffer aligned by VLENbytes
 *                                  2. w1, w2 - generated with qhdsp_hvx_2D_fft_gen_twiddles_real_vcf() function
 *                                  3. N is power of 2 and N>=64
 *                                  4. 2D IFFT assumes width=height=stride -> N
 */
int32_t qhdsp_hvx_r2difft_af(const float complex *input, uint32_t N, const float complex *w1, const float complex *w2, float *output)
{
    uint32_t LOG2N = ct0(N);

    if(N < 64 || N > 4096 || 1 << LOG2N != N)      // check size limits and if N is power of 2
    {
        return -1;
    }

    // temporary buffer for intermediate data storage
    float complex *temp_buff = (float complex *)memalign(VLEN, N*N*sizeof(float complex));
    float complex *input_ptr = (float complex *)input;
    float complex *scratch_buff = (float complex *)memalign(VLEN, N/2*sizeof(float complex));

    if(temp_buff == NULL || scratch_buff == NULL)
    {
        //printf("Unable to allocate temp buff... exiting.\n");
        return -1;
    }

    float complex *temp_buff_ptr = temp_buff;

    //                                stride        width        height
    uint32_t L2FETCH_REGISTER_ROWS = (VLEN << 16) | (VLEN << 8) | (N * 1/16);

    // first do all columns
    for(uint32_t i=0; i<N; i+=32)
    {
        qhdsp_hvx_c1difft_column_af(input_ptr, N, w1, temp_buff_ptr);
        input_ptr += 32;
        temp_buff_ptr += 32;
    }

    temp_buff_ptr = temp_buff;

    // ...then all rows
    for(uint32_t i=0; i<N; i++)
    {
        if(N-i > 1)
        {
            L2FETCH(temp_buff_ptr+N, L2FETCH_REGISTER_ROWS);
        }

        qhdsp_hvx_r1difft_af_scratch(temp_buff_ptr, N, w1, w2, output, scratch_buff);
        temp_buff_ptr += N;
        output+=N;
    }

    free(temp_buff);
    free(scratch_buff);

    return 0;
}

/**
 * @brief           [HVX] Real 1D 2^N half precision float point FFT
 * @param[in]       input - input samples in time domain (real)
 * @param[in]       N - number of samples on which FFT is performed
 * @param[in]       Wt1 - twiddle factors - for N/2-point complex FFT
 * @param[in]       Wt2 - twiddle factors - for last stage
 * @param[out]      output - FFT output buffer
 * @note
 *                  - Assumptions:
 *                                  1. input, Wt1, Wt2, output - buffer aligned by VLENbytes
 *                                  2. Wt1, Wt2 - generated with qhdsp_hvx_fft_gen_twiddles_real_vchf() function
 *                                  3. N is power of 2 and N>=VLEN
 */
int32_t qhdsp_hvx_r1dfft_ahf(const __fp16* input, uint32_t N, const qhl_cfloat16_t *Wt1, const qhl_cfloat16_t *Wt2, qhl_cfloat16_t *output)
{
    // Construct complex array with even
    // input as real and odd input as
    // imaginary parts
    // Then do N/2-point complex FFT
    qhdsp_hvx_c1dfft_ahf((qhl_cfloat16_t *)input, N/2, Wt1, output);

    // Calculate last stage butterflies
    // calculate FFT at k=0, k=N/2

    HVX_Vector output_0_backup = Q6_V_vsplat_R(*(uint32_t *)output);
    HVX_Vector output_N_4_backup = Q6_V_vsplat_R(*(uint32_t *)(&output[N/4]));

    HVX_VP V1__0, V3__2, V5__4, V7__6, V9__8, V11__10, V13__12, V15__14;
    HVX_Vector previous_output_0 = Q6_V_vzero();

    V4 = Q6_Vb_vsplat_R(0x7c);                      // VDELTA control for reversing 4-byte words in a vector
    V12 = Q6_Vh_vsplat_R(0x8000);                   // mask for negation - done with XOR operation

    V14 = Q6_Vh_vsplat_R(0x3800);                   // 0.5 in IEEE hf float
    V13 = Q6_V_vzero();                             // 0.0 * i
    V15_14 = Q6_W_vshuff_VVR(V13, V14, -2);         // shuffle real & imag parts

    HVX_Vector *out_out_0  = (HVX_Vector *)(&output[0]);
    HVX_Vector *out_in_N_2 = (HVX_Vector *)(&output[N / 2]);
    out_in_N_2--;       // move back pointer for first read

    HVX_Vector *twiddles   = (HVX_Vector *)Wt2;

    for (uint32_t i = 0; i < N / 4; i+=32)
    {
        V0 = Q6_V_vdelta_VV(*out_in_N_2, V4);
        V3_2 = Q6_W_vdeal_VVR(V1, V0, -2);          // split to real & imag parts (output[N/2 - i])

        V3 = Q6_V_vxor_VV(V3, V12);                 // negate imag part - conjugate
        V3_2 = Q6_W_vshuff_VVR(V3, V2, -2);         // shuffle back real & imag parts

        V0 = *out_out_0++;                          // output[i]; i:  0-31
        V1 = *out_out_0--;                          // output[i]; i: 32-63

        V0 = Q6_V_valign_VVR(V1, V0, 4);            // output[i]; i:  1-32

        V6 = Q6_Vqf16_vadd_VhfVhf(V0, V2);              // X = Z[i] + conj_f(Z[N / 2 - i]);
        V8 = Q6_Vqf16_vsub_VhfVhf(V0, V2);              // Y = Z[i] - conj_f(Z[N / 2 - i]);

        qf16_V_CPLX_MULT_qf16_hf_vect(V6, V14, &V6); // X = 0.5 * X;

        V10 = *twiddles++;

        qf16_V_CPLX_MULT_qf16_hf_vect(V8, V10, &V8); // Y = Wt2[i - 1] * Y;

        V0 = Q6_Vqf16_vadd_Vqf16Vqf16(V6, V8);                  // X + Y;
        V2 = Q6_V_valign_VVR(V0, previous_output_0, 124);
        previous_output_0 = V0;
        *out_out_0++ = Q6_Vhf_equals_Vqf16(V2);         // convert to IEEE float; output[i] = X + Y;

        V10 = Q6_Vqf16_vsub_Vqf16Vqf16(V6, V8);                 // X - Y;
        V10 = Q6_Vhf_equals_Vqf16(V10);                 // convert to IEEE float; X - Y;

        V11_10 = Q6_W_vdeal_VVR(V11, V10, -2);      // split to real & imag parts
        V11 = Q6_V_vxor_VV(V11, V12);               // negate imag part - conjugate: conj_f(X - Y);
        V1_0 = Q6_W_vshuff_VVR(V11, V10, -2);       // shuffle back real & imag parts

        V0 = Q6_V_vdelta_VV(V0, V4);

        *out_in_N_2-- = V0;                         // output[N / 2 - i] = conj_f(X - Y);
    }

    // ---------------------------
    // output[0]

    // X = creal_f(output_0_backup) + 1i * 0;
    // Y = cimag_f(output_0_backup) + 1i * 0;
    // output[0] = X + Y;

    HVX_VectorPred Q1 = Q6_Q_vsetq_R(1*sizeof(qhl_cfloat16_t));

    V1_0 = Q6_W_vdeal_VVR(output_0_backup, output_0_backup, -2);      // split to real & imag parts
    V2 = Q6_Vqf16_vadd_VhfVhf(V1, V0);
    V2 = Q6_Vhf_equals_Vqf16(V2);                       // convert to IEEE float;
    V1_0 = Q6_W_vshuff_VVR(V13, V2, -2);            // shuffle real & imag parts
    Q6_vmem_QRIV(Q1, (HVX_Vector*)&output[0], V0);
    // ---------------------------

    // ---------------------------
    // output[N/4]
    /*
        output[N / 4] calculation:

        X = Z[N / 4] + conj_f(Z[N / 2 - N / 4]) = Z[N / 4] + conj_f(Z[N / 4]) = 2 * Z[N / 4].re + 1i * 0;
        Y = Z[N / 4] - conj_f(Z[N / 2 - N / 4]) = Z[N / 4] - conj_f(Z[N / 4]) = 0.0 + 1i * 2.0 * Z[N / 4].im;
        X = 0.5 * X = Z[N / 4].re + 1i * 0;
        Y = Wt2[i - 1] * Y;
        output[i] = X + Y;
    */

    // X = creal_f(output_N_4_backup) + 1i * 0;
    // Y = 0.0 + 2.0 * 1i * cimag_f(output_N_4_backup);

    V1_0 = Q6_W_vdeal_VVR(output_N_4_backup, output_N_4_backup, -2);      // split to real & imag parts
    V3_2 = Q6_W_vshuff_VVR(V13, V0, -2);            // shuffle real & imag parts - X [hf]

    V6 = Q6_Vh_vsplat_R(0x4000);                    // 2.0 in IEEE hf float

    V10 = Q6_Vqf16_vmpy_VhfVhf(V1, V6);
    V11_10 = Q6_W_vshuff_VVR(V10, V13, -2);         // shuffle real & imag parts - Y [qf16]

    HVX_Vector Wt2_N_4_m1 = Q6_V_vsplat_R(*(uint32_t *)(&Wt2[N/4-1]));

    V7_6 = Q6_W_vcombine_VV(Wt2_N_4_m1, Wt2_N_4_m1);

    hf_V_CPLX_MULT_qf16_hf(V11_10, V7_6, &V1_0);

    V2 = Q6_Vqf16_vadd_VhfVhf(V0, V2);
    V2 = Q6_Vhf_equals_Vqf16(V2);                       // convert to IEEE float;

    Q6_vmem_QRIV(Q1, (HVX_Vector*)&output[N/4], V2);
    // ---------------------------

    HVX_VectorPair *out_out_N_2 = (HVX_VectorPair *)(&output[N / 2]);
    HVX_Vector *out_in_N   = (HVX_Vector *)(&output[N/2]);

    previous_output_0 = *out_in_N--;                // output[i]; i: N/2 : N/2+31

    for (uint32_t i = N / 2; i < N; i+=64)
    {
        V0 = *out_in_N--;                           // output[i]; i: N/2-32 : N/2-1
        V1 = *out_in_N--;                           // output[i]; i: N/2-64 : N/2-33

        V2 = Q6_V_valign_VVR(previous_output_0, V0, 4); // output[i]; i: N/2-31 : N/2
        V3 = Q6_V_valign_VVR(V0, V1, 4);            // output[i]; i: N/2-63 : N/2-32

        V2 = Q6_V_vdelta_VV(V2, V4);                // output[i]; i: N/2 : N/2-31
        V3 = Q6_V_vdelta_VV(V3, V4);                // output[i]; i: N/2-32 : N/2-63
        previous_output_0 = V1;

        V3_2 = Q6_W_vdeal_VVR(V3, V2, -2);          // split to real & imag parts (output[N/2 - i])
        V3 = Q6_V_vxor_VV(V3, V12);                 // negate imag part
        V1_0 = Q6_W_vshuff_VVR(V3, V2, -2);         // shuffle back real & imag parts

        *out_out_N_2++ = V1_0;
    }

    // ---------------------------
    // output[N/2]
    // X = creal_f(output_0_backup_scalar) + 1i * 0;
    // Y = cimag_f(output_0_backup_scalar) + 1i * 0;
    // output[N/2] = X - Y;

    V1_0 = Q6_W_vdeal_VVR(output_0_backup, output_0_backup, -2);      // split to real & imag parts
    V2 = Q6_Vqf16_vsub_VhfVhf(V0, V1);
    V2 = Q6_Vhf_equals_Vqf16(V2);                       // convert to IEEE float;
    V1_0 = Q6_W_vshuff_VVR(V13, V2, -2);            // shuffle real & imag parts
    Q6_vmem_QRIV(Q1, (HVX_Vector*)&output[N/2], V0);
    // ---------------------------

    return 0;
}

/**
 * @brief           [HVX] Real 1D 2^N half precision float point IFFT
 * @param[in]       input - input samples in frequency domain
 * @param[in]       n - number of samples on which IFFT is performed
 * @param[in]       Wt1 - twiddle factors - for N/2-point complex IFFT
 * @param[in]       Wt2 - twiddle factors - for last stage
 * @param[out]      output - IFFT output buffer
 * @note
 *                  - Assumptions:
 *                                  1. input, w1, w2, output - buffer aligned by VLENbytes
 *                                  2. w1, w2 - generated with qhdsp_hvx_fft_gen_twiddles_real_vchf() function
 *                                  3. N is power of 2 and N>=VLEN
 */
int32_t qhdsp_hvx_r1difft_ahf(const qhl_cfloat16_t *input, uint32_t N, const qhl_cfloat16_t *Wt1, const qhl_cfloat16_t *Wt2, __fp16 *output)
{
    qhl_cfloat16_t *c1difft_hf_input_buff;

    const int32_t byte_alignment = VLEN * sizeof(uint8_t);
    if(N/2 * sizeof(qhl_cfloat16_t) + (byte_alignment-1) > 2048)
    {
        c1difft_hf_input_buff = (qhl_cfloat16_t *)memalign(byte_alignment, N/2*sizeof(qhl_cfloat16_t));
    }
    else
    {
        c1difft_hf_input_buff = (qhl_cfloat16_t *)__builtin_alloca(N/2*sizeof(qhl_cfloat16_t) + (byte_alignment-1));
        int32_t offset = byte_alignment - 1;
        c1difft_hf_input_buff = (qhl_cfloat16_t *)(((int32_t)c1difft_hf_input_buff + offset) & ~(byte_alignment-1));
    }

    if(c1difft_hf_input_buff==NULL)
    {
        //printf("Unable to allocate temp buff, exiting...");
        return -1;
    }

    HVX_VP V1__0, V3__2, V5__4, V7__6, V9__8, V11__10, V13__12, V15__14;

    HVX_Vector *in_0      = (HVX_Vector *)(&input[0]);
    HVX_Vector *in_N_2    = (HVX_Vector *)(&input[N / 2]);
    HVX_VectorPair *temp_buff_ptr;
    HVX_Vector *twiddles  = (HVX_Vector *)Wt2;

    V12 = Q6_Vh_vsplat_R(0x8000);                   // mask for negation - done with XOR operation
    V0 = Q6_V_vzero();                              // 0.0
    V1 = Q6_Vh_vsplat_R(0xb800);                    // -0.5 in IEEE hf float
    V3_2 = Q6_W_vshuff_VVR(V1, V0, -2);             // shuffle real & imag parts together: 0.0 - 0.5 * i
    HVX_Vector previous_twiddles = V2;

    V1 = Q6_Vh_vsplat_R(0x3800);                    // 0.5 in IEEE hf float
    V15_14 = Q6_W_vshuff_VVR(V0, V1, -2);           // shuffle real & imag parts together: 0.5 + 0.0 * i

    temp_buff_ptr = (HVX_VectorPair *)c1difft_hf_input_buff;

    HVX_Vector previous_in_N_2 = *in_N_2--;         // input[N/2]    -> input[N/2+15];

    V10 = Q6_Vb_vsplat_R(0x7c);                     // VDELTA control for reversing 4-byte words in a vector

    for (uint32_t i = 0; i < N / 2; i+=64)
    {
        V0 = *in_0++;                               // input[i]; i:  0-31
        V1 = *in_0++;                               // input[i]; i:  32-63

        V2 = *in_N_2--;                             // input[N/2-32] -> input[N/2-1]
        V3 = *in_N_2--;                             // input[N/2-64] -> input[N/2-33]

        V4 = Q6_V_valign_VVR(previous_in_N_2, V2, 4); // input[N/2-31] -> input[N/2-0]
        V5 = Q6_V_valign_VVR(V2, V3, 4);              // input[N/2-63] -> input[N/2-32]
        previous_in_N_2 = V3;

        V2 = Q6_V_vdelta_VV(V4, V10);               // input[N/2-0]  -> input[N/2-31]
        V3 = Q6_V_vdelta_VV(V5, V10);               // input[N/2-32] -> input[N/2-63]

        V3_2 = Q6_W_vdeal_VVR(V3, V2, -2);          // V2 - real part, V3 - imag part
        V3 = Q6_V_vxor_VV(V3, V12);                 // negate imag part - conjugate
        V3_2 = Q6_W_vshuff_VVR(V3, V2, -2);         // shuffle back real & imag parts together
                                                    // V3_2 = conjugate(input[N/2-i]); i:  0-31

        V4 = Q6_Vqf16_vadd_VhfVhf(V0, V2);              // X = input[i] + conj_f(input[N/2-i]); - 1st 32 output values
        V5 = Q6_Vqf16_vadd_VhfVhf(V1, V3);              // X = input[i] + conj_f(input[N/2-i]); - 2nd 32 output values

        V6 = Q6_Vqf16_vsub_VhfVhf(V0, V2);              // Y = input[i] - conj_f(input[N/2-i]); - 1st 32 output values
        V7 = Q6_Vqf16_vsub_VhfVhf(V1, V3);              // Y = input[i] - conj_f(input[N/2-i]); - 2nd 32 output values

        qf16_V_CPLX_MULT_qf16_hf(V5_4, V15_14, &V5_4);// X = 0.5 * X;

        V0 = *twiddles++;                           // w2[i];   i:  0-31
        V1 = *twiddles++;                           // w2[i];   i:  32-63

        V8 = Q6_V_valign_VVR(V0, previous_twiddles, 124);  // w2[i];   i:  1-63 & previous_twiddles (1 value)
        V9 = Q6_V_valign_VVR(V1, V0, 124);

        previous_twiddles = V1;

        qf16_V_CPLX_MULT_conj_qf16_hf(V7_6, V9_8, &V7_6);  // Y = Y * conj_f(Wt2[i-1]);

        V0 = Q6_Vqf16_vadd_Vqf16Vqf16(V4, V6);                  // X + Y;
        V1 = Q6_Vqf16_vadd_Vqf16Vqf16(V5, V7);                  // X + Y;
        V0 = Q6_Vhf_equals_Vqf16(V0);
        V1 = Q6_Vhf_equals_Vqf16(V1);

        *temp_buff_ptr++ = V1_0;
    }

    qhdsp_hvx_c1difft_ahf(c1difft_hf_input_buff, N/2, Wt1, (qhl_cfloat16_t *)output);

    if(N/2 * sizeof(qhl_cfloat16_t) + (byte_alignment-1) > 2048)
    {
        free(c1difft_hf_input_buff);
    }

    return 0;
}

/**
 * @brief           [HVX] Real 1D 2^N half precision float point IFFT - scratch
 *                  Temporary buffer provided from outside - improves performance
 *                  when called multiple times (such as in 2D).
 * @param[in]       input - input samples in frequency domain
 * @param[in]       n - number of samples on which IFFT is performed
 * @param[in]       Wt1 - twiddle factors - for N/2-point complex IFFT
 * @param[in]       Wt2 - twiddle factors - for last stage
 * @param[out]      output - IFFT output buffer
 * @param[in]       scratch_buff - temporary buffer (size=N/2*sizeof(qhl_cfloat16_t), alignment=VLENB)
 * @note
 *                  - Assumptions:
 *                                  1. input, w1, w2, output - buffer aligned by VLENbytes
 *                                  2. w1, w2 - generated with qhdsp_hvx_fft_gen_twiddles_real_vchf() function
 *                                  3. N is power of 2 and N>=VLEN
 */
int32_t qhdsp_hvx_r1difft_ahf_scratch(const qhl_cfloat16_t *input, uint32_t N, const qhl_cfloat16_t *Wt1, const qhl_cfloat16_t *Wt2, __fp16 *output, qhl_cfloat16_t *scratch_buff)
{
    HVX_VP V1__0, V3__2, V5__4, V7__6, V9__8, V11__10, V13__12, V15__14;

    HVX_Vector *in_0      = (HVX_Vector *)(&input[0]);
    HVX_Vector *in_N_2    = (HVX_Vector *)(&input[N / 2]);
    HVX_VectorPair *temp_buff_ptr;
    HVX_Vector *twiddles  = (HVX_Vector *)Wt2;

    V12 = Q6_Vh_vsplat_R(0x8000);                   // mask for negation - done with XOR operation
    V0 = Q6_V_vzero();                              // 0.0
    V1 = Q6_Vh_vsplat_R(0xb800);                    // -0.5 in IEEE hf float
    V3_2 = Q6_W_vshuff_VVR(V1, V0, -2);             // shuffle real & imag parts together: 0.0 - 0.5 * i
    HVX_Vector previous_twiddles = V2;

    V1 = Q6_Vh_vsplat_R(0x3800);                    // 0.5 in IEEE hf float
    V15_14 = Q6_W_vshuff_VVR(V0, V1, -2);           // shuffle real & imag parts together: 0.5 + 0.0 * i

    temp_buff_ptr = (HVX_VectorPair *)scratch_buff;

    HVX_Vector previous_in_N_2 = *in_N_2--;         // input[N/2]    -> input[N/2+15];

    V10 = Q6_Vb_vsplat_R(0x7c);                     // VDELTA control for reversing 4-byte words in a vector

    for (uint32_t i = 0; i < N / 2; i+=64)
    {
        V0 = *in_0++;                               // input[i]; i:  0-31
        V1 = *in_0++;                               // input[i]; i:  32-63

        V2 = *in_N_2--;                             // input[N/2-32] -> input[N/2-1]
        V3 = *in_N_2--;                             // input[N/2-64] -> input[N/2-33]

        V4 = Q6_V_valign_VVR(previous_in_N_2, V2, 4); // input[N/2-31] -> input[N/2-0]
        V5 = Q6_V_valign_VVR(V2, V3, 4);              // input[N/2-63] -> input[N/2-32]
        previous_in_N_2 = V3;

        V2 = Q6_V_vdelta_VV(V4, V10);               // input[N/2-0]  -> input[N/2-31]
        V3 = Q6_V_vdelta_VV(V5, V10);               // input[N/2-32] -> input[N/2-63]

        V3_2 = Q6_W_vdeal_VVR(V3, V2, -2);          // V2 - real part, V3 - imag part
        V3 = Q6_V_vxor_VV(V3, V12);                 // negate imag part - conjugate
        V3_2 = Q6_W_vshuff_VVR(V3, V2, -2);         // shuffle back real & imag parts together
                                                    // V3_2 = conjugate(input[N/2-i]); i:  0-31

        V4 = Q6_Vqf16_vadd_VhfVhf(V0, V2);              // X = input[i] + conj_f(input[N/2-i]); - 1st 32 output values
        V5 = Q6_Vqf16_vadd_VhfVhf(V1, V3);              // X = input[i] + conj_f(input[N/2-i]); - 2nd 32 output values

        V6 = Q6_Vqf16_vsub_VhfVhf(V0, V2);              // Y = input[i] - conj_f(input[N/2-i]); - 1st 32 output values
        V7 = Q6_Vqf16_vsub_VhfVhf(V1, V3);              // Y = input[i] - conj_f(input[N/2-i]); - 2nd 32 output values

        qf16_V_CPLX_MULT_qf16_hf(V5_4, V15_14, &V5_4);// X = 0.5 * X;

        V0 = *twiddles++;                           // w2[i];   i:  0-31
        V1 = *twiddles++;                           // w2[i];   i:  32-63

        V8 = Q6_V_valign_VVR(V0, previous_twiddles, 124);  // w2[i];   i:  1-63 & previous_twiddles (1 value)
        V9 = Q6_V_valign_VVR(V1, V0, 124);

        previous_twiddles = V1;

        qf16_V_CPLX_MULT_conj_qf16_hf(V7_6, V9_8, &V7_6);  // Y = Y * conj_f(Wt2[i-1]);

        V0 = Q6_Vqf16_vadd_Vqf16Vqf16(V4, V6);                  // X + Y;
        V1 = Q6_Vqf16_vadd_Vqf16Vqf16(V5, V7);                  // X + Y;
        V0 = Q6_Vhf_equals_Vqf16(V0);
        V1 = Q6_Vhf_equals_Vqf16(V1);

        *temp_buff_ptr++ = V1_0;
    }

    qhdsp_hvx_c1difft_ahf(scratch_buff, N/2, Wt1, (qhl_cfloat16_t *)output);

    return 0;
}

/**
 * @brief           [HVX] Real 2D (NxN) 2^N half precision float point FFT
 * @param[in]       input - input samples
 * @param[in]       N - number of points (of both dimensions) on which FFT is performed
 * @param[in]       w1 - twiddle factors - for N/2-point complex FFT
 * @param[in]       w2 - twiddle factors - for last stage
 * @param[out]      output - FFT output buffer
 * @param[in]       scale_factor - since output values of FFT function can have large range (example: if inputs are in range
 *                  from -1.0 to 1.0 for 128x128 FFT outputs can go up to 16384.0 = pow(2,log2(128)+log2(128)) ) scale_factor
 *                  should be used to multiply (scale down) inputs before applying 2D FFT. This way FFT outputs will be scaled
 *                  down by the same value as input was scaled down, but if properly set, FFT outputs won't get to +/-INF value
 *                  (for IEEE half precision float point every value above 65519 is INF). If scaling down is not needed, simply
 *                  pass 1.0 value in HF (0x3c00) as input parameter.
 * @note
 *                  - Assumptions:
 *                                  1. input, w1, w2, output - buffer aligned by VLENbytes
 *                                  2. w1, w2 - generated with qhdsp_hvx_2D_fft_gen_twiddles_real_vchf() function
 *                                  3. N is power of 2 and N>=64
 *                                  4. 2D FFT assumes width=height=stride -> N
 */
int32_t qhdsp_hvx_r2dfft_ahf(const __fp16 *input, uint32_t N, const qhl_cfloat16_t *w1, const qhl_cfloat16_t *w2, qhl_cfloat16_t *output, const __fp16 *scale_factor)
{
    uint32_t LOG2N = ct0(N);

    if(N < 64 || N > 4096 || 1 << LOG2N != N)      // check size limits and if N is power of 2
    {
        return -1;
    }

    // temporary buffer for intermediate data storage
    qhl_cfloat16_t *temp_buff = (qhl_cfloat16_t *)memalign(VLEN, N*N*sizeof(qhl_cfloat16_t));
    __fp16 *input_ptr = (__fp16 *)input;

    if(temp_buff == NULL)
    {
        //printf("Unable to allocate temp buff... exiting.\n");
        return -1;
    }

    qhl_cfloat16_t *temp_buff_ptr = temp_buff;

    //                                stride        width        height
    uint32_t L2FETCH_REGISTER_ROWS = (VLEN << 16) | (VLEN << 8) | (N * 1/32);

    HVX_Vector scale_factor_vect = Q6_Vh_vsplat_R(*(uint16_t *)scale_factor);
    HVX_Vector* input_ptr_vect = (HVX_Vector*)input;
    HVX_Vector scaled_input_vect;

    // first do all rows
    // place results in bit-reverse order
    for(uint32_t i=0; i<N; i++)
    {
        if(N-i > 1)
        {
            L2FETCH(input_ptr+N, L2FETCH_REGISTER_ROWS);
        }

        // scale inputs
        for(uint32_t j=0; j<N; j+=64)
        {
            scaled_input_vect = Q6_Vqf16_vmpy_VhfVhf(*input_ptr_vect, scale_factor_vect);
            *input_ptr_vect++ = Q6_Vhf_equals_Vqf16(scaled_input_vect);
        }

        qhdsp_hvx_r1dfft_ahf(input_ptr, N, w1, w2, &temp_buff[bitrev(i, LOG2N)*N]);

        input_ptr += N;
    }

    // ...then all columns
    for(uint32_t i=0; i<N; i+=64)
    {
        qhdsp_hvx_c1dfft_column_ahf(temp_buff_ptr, N, w1, output);

        temp_buff_ptr += 64;
        output += 64;
    }

    free(temp_buff);

    return 0;
}

/**
 * @brief           [HVX] Real 2D (NxN) 2^N half precision float point IFFT
 * @param[in]       input - input samples
 * @param[in]       N - number of points (of both dimensions) on which IFFT is performed
 * @param[in]       w1 - twiddle factors - for N/2-point complex IFFT
 * @param[in]       w2 - twiddle factors - for last stage
 * @param[out]      output - IFFT output buffer
 * @param[in]       scale_factor - if FFT inputs were scaled down, this parameter is used to scale up IFFT outputs (by reciprocal
 *                  value from the one used in FFT function). If scaling down wasn't done in FFT function, simply pass 1.0 value
 *                  in HF (0x3c00) as input parameter.
 * @note
 *                  - Assumptions:
 *                                  1. input, w1, w2, output - buffer aligned by VLENbytes
 *                                  2. w1, w2 - generated with qhdsp_hvx_2D_fft_gen_twiddles_real_vchf() function
 *                                  3. N is power of 2 and N>=64
 *                                  4. 2D IFFT assumes width=height=stride -> N
 */
int32_t qhdsp_hvx_r2difft_ahf(const qhl_cfloat16_t *input, uint32_t N, const qhl_cfloat16_t *w1, const qhl_cfloat16_t *w2, __fp16 *output, const __fp16 *scale_factor)
{
    uint32_t LOG2N = ct0(N);

    if(N < 64 || N > 4096 || 1 << LOG2N != N)      // check size limits and if N is power of 2
    {
        return -1;
    }

    // temporary buffer for intermediate data storage
    qhl_cfloat16_t *temp_buff = (qhl_cfloat16_t *)memalign(VLEN, N*N*sizeof(qhl_cfloat16_t));
    qhl_cfloat16_t *input_ptr = (qhl_cfloat16_t *)input;
    qhl_cfloat16_t *scratch_buff = (qhl_cfloat16_t *)memalign(VLEN, N/2*sizeof(qhl_cfloat16_t));

    if(temp_buff == NULL || scratch_buff == NULL)
    {
        //printf("Unable to allocate temp buff... exiting.\n");
        return -1;
    }

    qhl_cfloat16_t *temp_buff_ptr = temp_buff;

    //                                stride        width        height
    uint32_t L2FETCH_REGISTER_ROWS = (VLEN << 16) | (VLEN << 8) | (N * 1/32);

    HVX_Vector scale_factor_vect = Q6_Vh_vsplat_R(*(uint16_t *)scale_factor);
    HVX_Vector* output_ptr_vect = (HVX_Vector*)output;
    HVX_Vector scaled_output_vect;

    // first do all columns
    // place results in bit-reverse order
    for(uint32_t i=0; i<N; i+=64)
    {
        qhdsp_hvx_c1difft_column_ahf(input_ptr, N, w1, temp_buff_ptr);
        input_ptr += 64;
        temp_buff_ptr += 64;
    }

    temp_buff_ptr = temp_buff;

    // ...then all rows
    for(uint32_t i=0; i<N; i++)
    {
        if(N-i > 1)
        {
            L2FETCH(temp_buff_ptr+N, L2FETCH_REGISTER_ROWS);
        }

        qhdsp_hvx_r1difft_ahf_scratch(temp_buff_ptr, N, w1, w2, output, scratch_buff);

        // scale outputs
        for(uint32_t j=0; j<N; j+=64)
        {
            scaled_output_vect = Q6_Vqf16_vmpy_VhfVhf(*output_ptr_vect, scale_factor_vect);
            *output_ptr_vect++ = Q6_Vhf_equals_Vqf16(scaled_output_vect);
        }

        temp_buff_ptr += N;
        output+=N;
    }

    free(temp_buff);
    free(scratch_buff);

    return 0;
}

#endif /* #if __HVX_ARCH__ >= 68 */
